\section{Controller overview}
\subsection{General structure}
The robot controller has been structured according to the sense-think-act paradigm.

In fact, at each time step, the robots will:
\begin{enumerate}
  \item \emph{(Sense)} - Read the informations collected by the available sensors.
  \item \emph{(Think)} - Determine the values to send to the actuators according to the state machine defined in \nameref{sec:sm} and the information from the sensors.
  \item \emph{(Act)} - Control the actuators.
\end{enumerate}

In terms of code:
\begin{enumerate}
  \item \emph{(Sense)} - The \emph{ParseX} functions are used to read the values of the different sensors (proximity, ground, distance scanner and Range and Bearing) and provide information to the following step in a suitable form (e.g. repulsion vector or beacon table).
  \item \emph{(Think)} - The core of the behavior is implemented as a finite state machine (FSM), where each state is implemented as function.
  \item \emph{(Act)} - According to the position, and the control values computed in the previous step, determine the speeds to actuate on the wheels and the information to broadcast using 
  the Range and Bearing System.
\end{enumerate}

\subsection{Potential field approach}
Researchers in Swarm Robotics try to understand how a behavior at the swarm level could emerge from local interactions at the robot level, using information either coming from the environment or from other robots.

Among the different models of these interactions that can be found in the literature, I decided to apply the potential-field approach (cf. \cite{howard2002mobile}), based on the readings from the sensors.

These virtual potential fields $U_i$ are defined on the whole environment and robots are able to compute locally the force $\mathbf{F}_i$ resulting from the interaction with the corresponding field as:
\begin{equation}
  \mathbf{F}_i = -\nabla U_i
\end{equation}
With this approach, it is possible to explicitly construct a field $U_i$ to induce a certain behavior on the robot.
For this purpose, the following virtual fields have been defined:
\begin{itemize}
  \item Obstacle avoidance - $U_{obs}$
  \item Distance scanner - $U_{ds}$
  \item Range and Bearing - $U_{rab}$
\end{itemize} 

The resulting forces from the aforementioned vector fields are determined from the corresponding sensor readings.

That is, each reading is transformed into a vector whose angle and magnitude depends on the value of the reading itself.
All the vector are expressed in polar coordinates, in the form ($\rho$\emph{(Magnitude)},$\alpha$\emph{(Angle)}).
Note that $\mathbf{F_{obs}}$ and $\mathbf{F_{ds}}$ are repulsive, while $\mathbf{F_{rab}}$ 
is attractive and that $\mathbf{F_{obs}}$ is magnitude is twice as big as those of $\mathbf{F_{ds}}$ and $\mathbf{F_{rab}}$.

\paragraph{Obstacle avoidance}
The readings from the proximity sensors could be expressed as vectors:
\begin{equation} \label{eq:pi}
\mathbf{p_i} = (r_{prox_{i}},\theta_i)  
\end{equation}
where $r_{prox_i}$ corresponds to the value of the reading of the $i^{th}$ proximity sensor and $\theta_i$ is the corresponding angle.

The resulting force $\mathbf{F_{obs}}$ is then computed by summing up all the vectors obtained in \ref{eq:pi} and rescaling the result:
\begin{align}
\mathbf{sp} = \sum_i -\mathbf{p_i} & & \mathbf{F_{obs}} = (2.0,\theta_{sp})   
\end{align}
where $\theta_{sp}$ corresponds to the angle of $\mathbf{sp}$ vector.

\paragraph{Distance scanner}
The same principle as above is applied to the distance scanner readings.

In addition, each reading value is normalized in the range $(4[cm],150[cm])$ 
i.e. (\emph{Lower bound short range readings}, \emph{Upper bound long range readings}).
By subtracting the normalized value to 1, one is able to obtain a vector whose 
magnitude is increases linearly as the distance from the obstacle decrease.

\begin{equation}
\mathbf{ds_i} = (1 - \frac{150 - r_{ds_{i}}}{150-4},\theta_i)  
\end{equation}
where $r_{prox_i}$ corresponds to the value of the $i^{th}$ reading of the distance and $\theta_i$ is the corresponding angle.

Again, the resulting force $\mathbf{F_{ds}}$ is obtained by summing and rescaling the readings' vectors: 
\begin{align}
\mathbf{sds} = \sum_i -\mathbf{ds_i} & & \mathbf{F_{ds}} = (1.0,\theta_{sds})   
\end{align}
where $\theta_{sds}$ corresponds to the angle of $\mathbf{sds}$ vector.

\paragraph{Range and Bearing}
\begin{equation}
\mathbf{F_{rab}} = (1.0,\theta_{rab})  
\end{equation}
where $\theta_{rab}$ corresponds to the angle of RAB reading.

\subsection{General idea}
The general idea of the method is that the robots should first quit the initial deployment room, characterized by four surrounding walls, one of them containing an opening to let the robots move outside, and a dark grey floor, to then start the exploration phase required to find the target spots.

In order to reduce the interference phenomenon at the exit of the nest, a sequential deployment mechanism based on the robot id has been developed.

After exiting the nest, the agents should explore the environment, either individually (as \emph{explorers}) or using the collectively-gathered knowledge of the environment (i.e. the chain).

If no information is available, the robot could decide (stochastically) to become a starting point for a new chain in the environment.

Otherwise, the exploration of the environment should, in principle, profit of the already available knowledge.

Nevertheless, this method achieves the required chain formation behavior using the embodied information in the chain only to exist the nest, while the successive exploration is simply guided by the proximity sensors and the distance scanner.
 

\paragraph{Rules}\label{par:rules}
The actual chain formation behavior is driven by five simple rules:
\begin{enumerate}
  \item If the \textbf{nest} has been \textbf{left}, and \textbf{no} chain beacon have been already \textbf{sensed}, after $t_{ns}$ time step, decide with probability $p_{btoe}$ to stop and become a \emph{chain end (E)}.
  \item If the \textbf{nest} has been \textbf{left}, and \textbf{exactly one} chain beacon has been \textbf{sensed} at a distance greater than $d_{chain}$, stop and become a \emph{chain end (E)}.
  \item If a \textbf{chain end (E)} has \textbf{more than one} neighboring\textbf{ beacon}, but \textbf{less than three}, then it changes its state to \emph{chain member (M)}.
  \item If a \textbf{chain member (M)} has \textbf{more than two} neighboring \textbf{beacons}, then it changes its state to \emph{chain junction (J)}.
  \item The chain identifier of a new beacon is determined by incrementing of one unit the chain id of the closest beacon.
\end{enumerate}

\begin{center}
\begin{tikzpicture}[shorten >=1pt,node distance=3cm,on grid,auto] 
   \node[state,accepting] (R1)   {$E_0$}; 
   \node[state,thick] (R2) [right=of R1] {$M_1$};
   \node[state,thick] (R3) [right=of R2] {$J_2$};
   \node[state,accepting,thick] (R4) [above right=of R3] {$E_3$};
   \node[state,thick] (R5) [below right=of R3] {$M_3$};
   \node[state,accepting,thick] (R6) [right=of R5] {$E_4$};
    \path[]
    (R1) edge (R2)
    (R2) edge (R3)
    (R3) edge (R4)
    (R3) edge (R5)
    (R5) edge (R6);
\end{tikzpicture}
\captionof{figure}{Chain example with nodes labeling and id}
\end{center}

\subsection{References}

Concerning the chain formation rules, the main sources of inspiration were \cite{nouyan2004chain}, \cite{nouyan2008path} which helped me to better understand the chain structure (in particular, how to distinguish elements in the chain) and the chain navigation behavior (which have not been implemented here) and \cite{goss1992harvesting} for defining the conditions to extend the chain.

Furthermore, the idea of an incremental deployment has been taken from \cite{stirling2013energy}.

While in \cite{stirling2013energy} the incremental deployment was used to limit energy consumption in the deployment phase, here the same idea is used to prevent the interference phenomenon that could occur at the nest exit.

In fact, whenever a relevant number of robots (10+) tries to exit the nest at the same time, each agent will spend more time performing obstacle avoidance with respect to one another, rather than actually exiting the nest.


%\begin{equation}
  %\text{Ticks per complete revolution} = \frac{\pi [rad]}{\omega [\frac{rad}{s}]} \cdot tps [\frac{tick}{s}]
%\end{equation}


% \subsubsection{Logical FSM}

% \begin{center}
% \begin{tikzpicture}[shorten >=1pt,node distance=5cm,on grid,auto] 
%    \node[state,initial] (Ex)   {Exploration}; 
%    \node[state] (As) [below right=of Ex] {Assessing cluster}; 
%    \node[state] (Uw) [below left=of Ex] {Unit work};
%    \node[state] (De) [below left=of As] {Decision phase};
%     \path[->] 
%     (Ex) edge [bend left]  node  {Sense light} (As)
%     (As) edge [bend left]  node  {In sensing range} (De)
%     (De) edge node [right]  {Full cluster} (Ex) 
%         edge [bend left] node {Empty stations} (Uw)
%     (Uw) edge [bend left]  node [above right]  {End work} (De) ;
% \end{tikzpicture}
% \captionof{figure}{Logical FSM for the E-puck behavior}
% \end{center}

% \subsubsection{Real FSM}

 

% \subsection{TAM}
% The task are represented by spatially distributed booths.

% The robots must reach and enter the booth to undertake the corresponding working activity.


% \subsubsection{Logical FSM}

% \begin{center}
% \begin{tikzpicture}[shorten >=1pt,node distance=3cm,on grid,auto] 
%    \node[state,initial,thick,draw=green!75,fill=green!20,] (Av)   {Available}; 
%    \node[state,thick,draw=red!75,fill=red!20] (Oc) [below right=of Av] {Occupied}; 
%    \node[state,thick,draw=yellow!75,fill=yellow!20] (Un) [below left=of Av] {Unavailable};
%     \path[->] 
%     (Av) edge [bend left]  node  {Sense Robot} (Oc)
%     (Oc) edge [bend left]  node  {$T_w$ expired} (Un)
%     (Un) edge [bend left] node [left]  {Robot not sensed} (Av);
% \end{tikzpicture}
% \captionof{figure}{Logical and Implemented FSM for the TAM behavior}
% \end{center}
